/***************************************************************************
 创建者: 华磊
 开始时间: 2019.3.11
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)                                                                       *
 ***************************************************************************/

#include "modbusmanager.h"
#include "modbusmaster.h"
#include "modbusserver.h"
#include "domparser.h"
#include "GeneralDefine.h"
#include "messagelog.h"
#include "powermanager.h"
#include "fleetservice.h"
#include "modbus.h"

#define D_COMMAND_PARAMETER_SIZE 18

extern FleetService *fleetService;

ModbusManager::ModbusManager(QString configFileIn, PowerManager *powerManageIn)
{
    isInitialOkStatus=false;
    powerManage=powerManageIn;
    modbusTcpServerDafulatId=0xff;
    maxFleetCatchLineForDebug=15;
    isUseModbusFleet=0;

    //fleet
    address_currentPointRemainDistance=15;
    address_currentPointRemainAngle=17;
    address_currentPos_x=18;
    address_currentPos_y=20;
    address_currentPos_r=22;
    address_lineSpeed=24;
    address_angleSpeed=25;
    address_fleetStatus=26;
    address_loadStatus=27;
    address_executedCommandSerialId=28;
    address_passedStationId=29;
    fixed_address_setDo=56;

    modbusRtuMaster0=NULL;
    modbusRtuMaster0_currentSlaveId=-1;

    externTcpServerDafulatId=1;

    //DirectConnection 在发送者线程执行。BlockingQueuedConnection　在接收者线程执行（发送者柱塞等待）。
    qDebug() << "++++++++++++ModbusManager Start connect flag: "
             << QObject::connect(this, SIGNAL(createModbusTcpServerSignal(int,int)),
                                 this, SLOT(createModbusTcpServerSlot(int,int)),Qt::BlockingQueuedConnection);

    if(1==initialConfigFile(configFileIn))
    {
        //modbus调度服务
        if(true==isUseSystemInfoMap)
        {
            if(1!=initialModbusServer(modbusServerType,modbusServerAdress))//开启一个modbus服务
            {
                   addMsg( ENUM_MSG_ERROR,  "ModbusManager", "ModbusManager",  12401,  -1);

            }

        }

        //modbus robot register服务
        initialRobotModbusServer();


        timer=new QTimer();
        connect( timer, SIGNAL(timeout()), this, SLOT(timeOut_slot()) );
        timer->start( timerPeriod );//90ms处理一条请求。

        isInitialOkStatus=true;

    }
    else
    {
        addMsg( ENUM_MSG_ERROR,  "ModbusManager", "ModbusManager",  12400,  -1);
    }
}

int ModbusManager::getFleetCommandLog(std::queue<std::string> &fleetCommandLogOut)
{
    fleetLogMutex.lock();
    fleetCommandLogOut=fleetCommandLog;
    fleetLogMutex.unlock();
    return 1;
}

int ModbusManager::createModbusTcpServer(int portIn)
{
    //需要用信号槽的形式，否则modbus读写的相应事件在创建线程。
    emit createModbusTcpServerSignal(portIn,D_MODBUS_REGISTER_SIZE);
    if(!startFinishedStatus)
    {
        return -1;
    }
    return 1;
}


void ModbusManager::createModbusTcpServerSlot(int portIn, int regSizeIn)
{
    usingMutex.lock();
    if(true==modbusTcpServers.contains(portIn))
    {
        usingMutex.unlock();
        startFinishedStatus=true;
        return ;
    }
    ModbusConnection typeIn=Tcp;
    QString portNameIn="";
    SerialSettings serialSettingIn;
//    QString ip_port_in="127.0.0.1:";//使用192.xxxx访问不了。
    QString ip_port_in="localhost:";
    ip_port_in+=QString::number(portIn);
    ModbusServer *modbusServer = new ModbusServer(typeIn,portNameIn,serialSettingIn,ip_port_in,modbusTcpServerDafulatId,regSizeIn);

    //connect port
    if(1==modbusServer->connectPort())
    {
        modbusTcpServers.insert(portIn,modbusServer);
        qDebug()<<"ModbusManager::createModbusTcpServer connect success";
//        QMessageBox::information(NULL, "information", "connect success！", QMessageBox::Yes );
        usingMutex.unlock();
        startFinishedStatus=true;
        return ;

    }
    else
    {
        delete modbusServer;
        qDebug()<<"ModbusManager::createModbusTcpServerconnect failed";
//        QMessageBox::information(NULL, "information", "connect failed", QMessageBox::Yes );
        usingMutex.unlock();
        startFinishedStatus=false;
        return ;
    }
    usingMutex.unlock();
    startFinishedStatus=false;
    return ;
}

void ModbusManager::createModbusComServerSlot(int comIdIn)
{
    usingMutex.lock();
    if(true==modbusComServers.contains(comIdIn))
    {
        usingMutex.unlock();
        startFinishedStatus=true;
        return ;
    }
    ModbusConnection typeIn=Serial;
    QString portNameIn="/dev/ttyS";
    portNameIn+=QString::number(comIdIn);
    SerialSettings serialSettingIn;//使用默认通讯
    serialSettingIn.parity = 0;//不校验
    serialSettingIn.baud = 9600;
    serialSettingIn.dataBits = 8;
    serialSettingIn.stopBits = 1;
    serialSettingIn.responseTime = 1000;
    serialSettingIn.numberOfRetries = 3;
    QString ip_port_in="";
    ModbusServer *modbusServer = new ModbusServer(typeIn,portNameIn,serialSettingIn,ip_port_in,
                                                  modbusTcpServerDafulatId,D_MODBUS_REGISTER_SIZE);

    //connect port
    if(1==modbusServer->connectPort())
    {
        modbusComServers.insert(comIdIn,modbusServer);
        qDebug()<<"ModbusManager::createModbusComServerSlot connect success";
//        QMessageBox::information(NULL, "information", "connect success！", QMessageBox::Yes );
        usingMutex.unlock();
        startFinishedStatus=true;
        return ;

    }
    else
    {
        delete modbusServer;
        qDebug()<<"ModbusManager::createModbusTcpServerconnect failed";
//        QMessageBox::information(NULL, "information", "connect failed", QMessageBox::Yes );
        usingMutex.unlock();
        startFinishedStatus=false;
        return ;
    }
    usingMutex.unlock();
    startFinishedStatus=false;
    return ;
}

void ModbusManager::timeOut_slot()
{
    if( 1==isUseModbusFleet)
    {
        sendBasicInfo(modbusServerType,modbusServerAdress.toInt());
        processModbusFleetRequest(modbusServerType,modbusServerAdress.toInt());
        processModbusFleetRequest_easyRegisterCommand(modbusServerType,modbusServerAdress.toInt());
    }

    exchangeRobotRegister();

}

int ModbusManager::createModbusTcpClient(QString ipAndPortIn)
{

}

int ModbusManager::deleteModbusTcpServer(int portIn)
{
    usingMutex.lock();
    if(false==modbusTcpServers.contains(portIn))
    {
        usingMutex.unlock();
        return 0;
    }
    modbusTcpServers.value(portIn)->disConnectPort();
    delete modbusTcpServers.value(portIn);
    modbusTcpServers.remove(portIn);
    usingMutex.unlock();
    return 1;
}

int ModbusManager::deleteModbusTcpClient(QString ipAndPortIn)
{

}

int ModbusManager::readModbusRegister_Server(bool isTcp, int portIn, int regAddIn, int &regValueOut)
{
    if(isTcp)
    {
        usingMutex.lock();
        if(false==modbusTcpServers.contains(portIn))
        {
            usingMutex.unlock();
            return 0;
        }
        quint16 valueReturn;
        modbusTcpServers.value(portIn)->getHoldingRegisters(regAddIn,valueReturn);
        regValueOut=valueReturn;
        usingMutex.unlock();
    }
    else
    {


        usingMutex.lock();
        if(false==modbusComServers.contains(portIn))
        {
            usingMutex.unlock();
            return 0;
        }
        quint16 valueReturn;
        modbusComServers.value(portIn)->getHoldingRegisters(regAddIn,valueReturn);
        regValueOut=valueReturn;
        usingMutex.unlock();
    }

    return 1;
}

int ModbusManager::writeModbusRegister_Server(bool isTcp,int portIn, int regAddIn, int regValueIn)
{
    if(isTcp)
    {

        usingMutex.lock();
        if(false==modbusTcpServers.contains(portIn))
        {
            usingMutex.unlock();
            return 0;
        }
        modbusTcpServers.value(portIn)->setHoldingRegisters(regAddIn,regValueIn);
        usingMutex.unlock();
    }
    else
    {
        usingMutex.lock();
        if(false==modbusComServers.contains(portIn))
        {
            usingMutex.unlock();
            return 0;
        }
        modbusComServers.value(portIn)->setHoldingRegisters(regAddIn,regValueIn);
        usingMutex.unlock();


    }

    return 1;
}

#define D_TRY_CLIENT_COUNT 10

int ModbusManager::readModbusRegister_TcpClient(QString ipAndPortIn, int regAddIn, int &regValueOut)
{
    usingMutex.lock();
    modbus_t *tcpClientOut;
    if(1!=getModbusTcpClient(ipAndPortIn,tcpClientOut))
    {
        addMsg( ENUM_MSG_ERROR,  "ModbusManager", "ModbusManager",  12413,  -1);
        usingMutex.unlock();
        return -1;
    }
    int tmpResult;
    int tmpTry=0;
    do{
       tmpTry++;
       tmpResult=read_registers_tcp(tcpClientOut,regAddIn,regValueOut);
    }while(1!=tmpResult && D_TRY_CLIENT_COUNT>tmpTry);
    if(1!=tmpResult)
    {
        addMsg( ENUM_MSG_ERROR,  "ModbusManager", "ModbusManager",  12414,  -1);
        usingMutex.unlock();
        return -2;
    }
    modbus_close(tcpClientOut);//超过超时时间才会释放端口
    modbus_free(tcpClientOut);
    usingMutex.unlock();
    return 1;

}

int ModbusManager::writeModbusRegister_TcpClient(QString ipAndPortIn, int regAddIn, int regValueIn)
{
    usingMutex.lock();
    modbus_t *tcpClientOut;
    if(1!=getModbusTcpClient(ipAndPortIn,tcpClientOut))
    {
        addMsg( ENUM_MSG_ERROR,  "ModbusManager", "ModbusManager",  12413,  -1);
        usingMutex.unlock();
        return -1;
    }
    int tmpResult;
    int tmpTry=0;
    do{
       tmpTry++;
       tmpResult=write_register_tcp(tcpClientOut,regAddIn,regValueIn);
    }while(1!=tmpResult && D_TRY_CLIENT_COUNT>tmpTry);
    if(1!=tmpResult)
    {
        addMsg( ENUM_MSG_ERROR,  "ModbusManager", "ModbusManager",  12415,  -1);
        usingMutex.unlock();
        return -2;
    }
    modbus_close(tcpClientOut);//超过超时时间才会释放端口
    modbus_free(tcpClientOut);
    usingMutex.unlock();
    return 1;
}

int ModbusManager::readModbusRegister_rtuClient(int slaveIdIn, int regAddIn, int &regValueOut)
{
    usingMutex.lock();

    int tmpResult;
    int tmpTry=0;
    do{
       tmpTry++;
       tmpResult=read_registers_rtu(slaveIdIn,regAddIn,regValueOut);
    }while(1!=tmpResult && D_TRY_CLIENT_COUNT>tmpTry);
    if(-100==tmpResult)
    {
        addMsg( ENUM_MSG_ERROR,  "ModbusManager", "ModbusManager",  12418,  -1);
        usingMutex.unlock();
        return -1;
    }
    else if(0>tmpResult)
    {
        addMsg( ENUM_MSG_ERROR,  "ModbusManager", "ModbusManager",  12416,  -1);
        usingMutex.unlock();
        return -2;
    }
    usingMutex.unlock();
    return 1;

}

int ModbusManager::writeModbusRegister_rtuClient(int slaveIdIn, int regAddIn, int regValueIn)
{
    usingMutex.lock();

    int tmpResult;
    int tmpTry=0;
    do{
       tmpTry++;
       tmpResult=write_register_rtu(slaveIdIn,regAddIn,regValueIn);
    }while(1!=tmpResult && D_TRY_CLIENT_COUNT>tmpTry);
    if(-100==tmpResult)
    {
        addMsg( ENUM_MSG_ERROR,  "ModbusManager", "ModbusManager",  12418,  -1);
        usingMutex.unlock();
        return -1;
    }
    else if(0>tmpResult)
    {
        addMsg( ENUM_MSG_ERROR,  "ModbusManager", "ModbusManager",  12417,  -1);
        usingMutex.unlock();
        return -2;
    }
    usingMutex.unlock();
    return 1;
}

int ModbusManager::read_registers_tcp(modbus_t *tcpClientIn,  int address, int &valueOut)
{
    if(NULL==tcpClientIn)
    {
        return -100;
    }

    uint16_t dataStream[2];
    int resultKey=modbus_read_registers(tcpClientIn,address,1,dataStream);
    if(-1!=resultKey)
    {
//                qDebug()<<"getPositionFeedback:modbus_read_registers sucess"<<resultKey;
        valueOut=dataStream[0];
        return 1;
    }
    else
    {
        return -1;
    }



    return -1;
}

int ModbusManager::write_register_tcp(modbus_t *tcpClientIn, int address, int value)
{
    if(NULL==tcpClientIn)
    {
        return -100;
    }
    //每次读写寄存器都需要重新connect,否则容易失败。
    int resultKey=modbus_write_register(tcpClientIn,address,value);
    if(-1!=resultKey)
    {
//                qDebug()<<"quickStopDriver:modbus_write_register sucess"<<resultKey;
        return 1;
    }
    else
    {
        return -1;
    }


    return 1;
}

int ModbusManager::getModbusTcpClient(QString ipAndPortIn,modbus_t *&tcpClientOut)
{
//    if(modbusTcpClients.contains(ipAndPortIn))
//    {
//        tcpClientOut=modbusTcpClients.value(ipAndPortIn);//断线后重新连接
//        if (modbus_connect(tcpClientOut) == -1)
//        {
//    //            qDebug()<<"Modbus485Driver::connectPort connect failed";
//            modbus_free(tcpClientOut);
//            tcpClientOut=NULL;
//            modbusTcpClients.remove(ipAndPortIn);
//            return -2;
//        }
//        modbus_set_response_timeout(tcpClientOut, 0, 3500000);
//        return 1;
//    }
    //提取ip　和　port
    QRegExp rx("(.*):(.*)");
//    programStr.indexOf(rx);

    bool isMatch=rx.exactMatch(ipAndPortIn);

    if(false==isMatch )
    {
        qDebug()<<"getModbusTcpClient error, false==isMatch";
        return -1;
    }

//    rx.cap(1).toStdString().data()
//    qDebug()<<rx.cap(1);
    QString tmpIp=rx.cap(1)+'\0';
    tcpClientOut = modbus_new_tcp(tmpIp.toStdString().data()
                                  ,rx.cap(2).toInt());//由于是tcp client 连接，在同一程序中相同的端口可以连接多次
    modbus_set_slave(tcpClientOut,externTcpServerDafulatId);
    if (modbus_connect(tcpClientOut) == -1)
    {
//            qDebug()<<"Modbus485Driver::connectPort connect failed";
        modbus_free(tcpClientOut);
        tcpClientOut=NULL;
        return -2;
    }
    else
    {
        modbus_set_response_timeout(tcpClientOut, 0, 3500000);
//            qDebug()<<"connect success";
//        modbusTcpClients.insert(ipAndPortIn,tcpClientOut);
        return 1;
    }
}


int ModbusManager::read_registers_rtu( int modbusId,int address,int &valueOut)
{

    if(1!=checkModbusMaster(modbusId))
    {
        return -100;
    }

    uint16_t dataStream[2];
    int resultKey=modbus_read_registers(modbusRtuMaster0,address,1,dataStream);
    if(-1!=resultKey)
    {
//                qDebug()<<"getPositionFeedback:modbus_read_registers sucess"<<resultKey;
        valueOut=dataStream[1];
        return 1;
    }
    else
    {
        return -1;
    }



    return -1;

}


int ModbusManager::write_register_rtu(int modbusId,int address,int value)
{

    if(1!=checkModbusMaster(modbusId))
    {
        return -100;
    }

//    uint16_t writeData[2];
//    writeData[0]=value;
    int resultKey=modbus_write_register(modbusRtuMaster0,address,value);
    if(-1!=resultKey)
    {
//                qDebug()<<"quickStopDriver:modbus_write_register sucess"<<resultKey;
        return 1;
    }
    else
    {
        return -1;
    }


    return 1;
}

int ModbusManager::checkModbusMaster(int modbusId)
{
    if(E_MODBUS_TYPE_TCP==comParameter_master0.modbusType)
    {
        return 1;
    }
    if(NULL==modbusRtuMaster0 && -1==modbusRtuMaster0_currentSlaveId)
    {
        int returnKey;
        returnKey=connectPort(modbusId);
        if(1==returnKey)
        {
            modbusRtuMaster0_currentSlaveId=modbusId;
            return 1;
        }
        else
        {
            return -1;
        }
    }
    else if(modbusId !=modbusRtuMaster0_currentSlaveId)
    {
        int returnKey;
        modbus_close(modbusRtuMaster0);
        modbus_free(modbusRtuMaster0);
        returnKey=connectPort(modbusId);
        if(1==returnKey)
        {
            modbusRtuMaster0_currentSlaveId=modbusId;
            return 1;
        }
        else
        {
            return -2;
        }
    }

    return 1;
}


int ModbusManager::connectPort(int modbusId)
{


    if(E_MODBUS_TYPE_TCP==comParameter_master0.modbusType)
    {
        modbusRtuMaster0 = modbus_new_tcp(comParameter_master0.ip.toStdString().data()
                                      ,comParameter_master0.port);//由于是tcp client 连接，在同一程序中相同的端口可以连接多次
        modbus_set_slave(modbusRtuMaster0,modbusId);
        if (modbus_connect(modbusRtuMaster0) == -1)
        {
//            qDebug()<<"Modbus485Driver::connectPort connect failed";
            modbus_free(modbusRtuMaster0);
            modbusRtuMaster0=NULL;
            return -2;
        }
        else
        {
            modbus_set_response_timeout(modbusRtuMaster0, 0, 500000);
//            qDebug()<<"connect success";
            return 1;
        }
    }
    else if(E_MODBUS_TYPE_RTU==comParameter_master0.modbusType)
    {

        modbusRtuMaster0 =modbus_new_rtu(comParameter_master0.serialDeviceName.toStdString().data(),
                                     comParameter_master0.baud, (*comParameter_master0.parity.toStdString().data()),
                                            comParameter_master0.data_bit, comParameter_master0.stop_bit);
        modbus_set_slave(modbusRtuMaster0,modbusId);
        if (modbus_connect(modbusRtuMaster0) == -1)
        {
//           qDebug()<<"Modbus485Driver::connectPort connect failed";
            modbus_free(modbusRtuMaster0);
            modbusRtuMaster0=NULL;
            return -2;
        }
        else
        {
            modbus_set_response_timeout(modbusRtuMaster0, 0, 500000);
//            qDebug()<<"connect success";
            return 1;
        }
    }
    return -1;
}



double ModbusManager::transToDistance(int hight, int low)
{
    int32_t newValue=(hight<<16)|low;//int 32bit
//    qDebug()<<sizeof(int);
    return newValue/100000.0;
}

double ModbusManager::transToAngle(int hight, int low)
{
    int32_t newValue=(hight<<16)|low;//int 32bit
//    qDebug()<<sizeof(int);
    return newValue/100.0;
}

int ModbusManager::saveFleetCommand(QString commandInfo, int serialCodeIn)
{
    fleetLogMutex.lock();
    QDateTime current_date_time =QDateTime::currentDateTime();
    QString tmpStr=current_date_time.toString("yyyy.MM.dd-hh:mm:ss");
    tmpStr+=commandInfo+" ,serialId "+QString::number(serialCodeIn);

    fleetCommandLog.push(tmpStr.toStdString());
    if(fleetCommandLog.size()>maxFleetCatchLineForDebug)
    {
        fleetCommandLog.pop();
    }
    fleetLogMutex.unlock();
    return 1;
}

int ModbusManager::processModbusFleetRequest(int modbusServerTypeIn,int modbusServerAdressIn)
{


    //第一次支持命令。
    int commandId;
    static int commandId_old=0;
    static int commandSerialId_old=0;
    int commandSerialId;
    int commandParameter1,commandParameter2;
    readModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn,address_command-1,commandSerialId);
    readModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn,address_command,commandId);
    readModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn,address_command+1,commandParameter1);
    readModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn,address_command+2,commandParameter2);
    switch(commandId)
    {
    case 0xa1:
    {
        if(0xa1!=commandId_old)
        {
            qDebug()<<"ModbusManager::processModbusFleetRequest"<<commandId; 
            int returnFlag;
            powerManage->setAllMsgClear(userInfo,returnFlag);
            powerManage->servoOnCurrentRobot();
            addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12402,  -1);
        }
        break;
    }
    case 0xa2:
    {
        if(0xa2!=commandId_old)
        {
            qDebug()<<"ModbusManager::processModbusFleetRequest"<<commandId;
            addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12403,  -1);
            powerManage->servoOffCurrentRobot();
        }
        break;
    }
    case 0xa3:
    {
        if(0xa3!=commandId_old)
        {
            qDebug()<<"ModbusManager::processModbusFleetRequest"<<commandId;
            addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12404,  -1);
            int returnFlag;
            powerManage->setAllMsgClear(userInfo,returnFlag);
            powerManage->setRunModel(userInfo,ENUM_CONTROL_AUTO,returnFlag);
        }
        break;
    }
    case 0xa4:
    {
        if(0xa4!=commandId_old)
        {
            qDebug()<<"ModbusManager::processModbusFleetRequest"<<commandId;
            addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12405,  -1);
            int returnFlag;
            powerManage->programRun(userInfo,returnFlag);
        }
        break;
    }
    case 0xa5:
    {
        if(0xa5!=commandId_old)
        {
            qDebug()<<"ModbusManager::processModbusFleetRequest"<<commandId;
            addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12406,  -1);
            int returnFlag;
            powerManage->programCease(userInfo,returnFlag);
        }
        break;
    }
    case 0xa6:
    {
        if(0xa6!=commandId_old)
        {
            qDebug()<<"ModbusManager::processModbusFleetRequest"<<commandId;
            addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12407,  -1);
            int returnFlag;
            powerManage->programReset(userInfo,returnFlag);
        }
        break;
    }
    case 0xa7:
    {
        if(0xa7!=commandId_old)
        {
            qDebug()<<"ModbusManager::processModbusFleetRequest"<<commandId;
            addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12409,  -1,commandId);

        }
        break;
    }
    case 0xa8:
    {
        if(0xa8!=commandId_old)
        {
            qDebug()<<"ModbusManager::processModbusFleetRequest"<<commandId<<commandParameter1;
            addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12408,  -1,commandParameter1);
            int returnFlag;
            if(1!=powerManage->initialVehiclePosition_byProgramPoint(userInfo, commandParameter1, returnFlag))
            {

            }
        }
        break;
    }
    case 0xa9:
    {
        if(0xa9!=commandId_old)
        {
            qDebug()<<"ModbusManager::processModbusFleetRequest"<<commandId<<commandParameter1;
            addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12420,  -1,commandParameter1);
            if(programNameList.contains(commandParameter1))
            {
                fleetService->loadInternalProgram(userInfo,mapList.value(commandParameter1).toStdString());
            }
            else
            {
                addMsg( ENUM_MSG_ERROR,  "ModbusManager", "ModbusManager",  12421,  -1,commandParameter1);
                break;
            }

            break;
        }
        break;
    }//new20200426
    case 0xaa:
    {
        if(0xaa!=commandId_old)
        {
            qDebug()<<"ModbusManager::processModbusFleetRequest"<<commandId;
            addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12426,  -1);
            int returnFlag;
            powerManage->setAllMsgClear(userInfo,returnFlag);
            powerManage->setRunModel(userInfo,ENUM_CONTROL_T1,returnFlag);
        }
        break;
    }
    case 0xab:
    {
        if(0xab!=commandId_old)
        {
            qDebug()<<"ModbusManager::processModbusFleetRequest"<<commandId;
            addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12427,  -1);
            int returnFlag;
            powerManage->setAllMsgClear(userInfo,returnFlag);
            powerManage->setRunModel(userInfo,ENUM_CONTROL_T2,returnFlag);
        }
        break;
    }
    case 0xac:
    {
        if(0xac!=commandId_old)
        {
            qDebug()<<"ModbusManager::processModbusFleetRequest"<<commandId;
            addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12428,  -1);
            int returnFlag;
            powerManage->setAllMsgClear(userInfo,returnFlag);
            powerManage->setRunModel(userInfo,ENUM_CONTROL_EXT,returnFlag);
        }
        break;
    }
    case 0xad:
    {
        if(0xad!=commandId_old)
        {
            qDebug()<<"ModbusManager::processModbusFleetRequest"<<commandId;
            addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12429,  -1);
            int returnFlag;
            powerManage->setAllMsgClear(userInfo,returnFlag);
            powerManage->setRunModel(userInfo,ENUM_CONTROL_FLEET,returnFlag);
        }
        break;
    }
    case 0xae:
    {
        if(0xae!=commandId_old)
        {
            qDebug()<<"ModbusManager::processModbusFleetRequest"<<commandId;
            addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12425,  -1);
            int returnFlag;
            powerManage->setUltrasonicSensorCheckEnable(userInfo,false,returnFlag);
            powerManage->setPointCloudCheckEnable(userInfo,false,returnFlag);

        }
        break;
    }
    case 0xaf:
    {
        if(0xaf!=commandId_old)
        {
            qDebug()<<"ModbusManager::processModbusFleetRequest"<<commandId;
            addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12424,  -1);
            int returnFlag;
            powerManage->setUltrasonicSensorCheckEnable(userInfo,true,returnFlag);
            powerManage->setPointCloudCheckEnable(userInfo,true,returnFlag);

        }
        break;
    }
    case 0xb0://jog
    {
        if(0xb0!=commandId_old)
        {
            qDebug()<<"ModbusManager::processModbusFleetRequest"<<commandId;
            addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12423,  -1);
            int returnFlag;
            double tmpSpeed=commandParameter2/100.0;
            switch(commandParameter1)
            {
            case 0:
                powerManage->vehicleJog(userInfo,1,tmpSpeed,0,0);
                break;
            case 1:
                powerManage->vehicleJog(userInfo,1,-tmpSpeed,0,0);
                break;
            case 2:
                powerManage->vehicleJog(userInfo,1,0,tmpSpeed,0);
                break;
            case 3:
                powerManage->vehicleJog(userInfo,1,0,-tmpSpeed,0);
                break;
            case 4:
                powerManage->vehicleJog(userInfo,1,0,0,tmpSpeed);
                break;
            case 5:
                powerManage->vehicleJog(userInfo,1,0,0,-tmpSpeed);
                break;
            default:
            {
                qDebug()<<"ModbusManager::processModbusFleetRequest error ,unknow jog method"<<commandParameter1;
                addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12422,  -1);
            }

            }


        }
        break;
    case 0xb1:
    {
        if(0xb1!=commandId_old)
        {
            qDebug()<<"ModbusManager::processModbusFleetRequest"<<commandId;
            addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12430,  -1);
            powerManage->jogStop(userInfo);

        }
        break;
    }
    }

    }
    commandId_old=commandId;



    if(ENUM_CONTROL_FLEET!=powerManage->getRunModel())
    {
        return 1;
    }
    //直接do写寄存器
    int tmpDoValue;
    readModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn,fixed_address_setDo,tmpDoValue);
    fleetService->setDoVector(tmpDoValue,doRegisterValidMask);

    //调度拓展命令
    if(commandSerialId_old==commandSerialId || 1!=isUseModbusFleet)
    {
        return 1;
    }

    QVector<int> commandInfo;
    commandInfo.resize(D_COMMAND_PARAMETER_SIZE);
    for(int i=0;i<commandInfo.size();i++)
    {
        readModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn,address_command+i,commandInfo[i]);
    }

    qDebug()<<"ModbusManager::processModbusFleetRequest"<<commandId;
    QString tmpCommandInfo;
    //新拓展调度命令
    switch(commandId)
    {
    case 0xc1://清除轨迹
    {

        tmpCommandInfo="clearPath ";
        fleetService->clearCommand(userInfo);
        break;
    }
    case 0xc2://路径
    {
        tmpCommandInfo="point ";
        LaserPathParameter paraIn;
        bool isParameterOk=true;
        int errorParameterIndex;
        //提取direction
        if(1==commandInfo[1])
        {
            paraIn.isPositiveDirection=true;
            tmpCommandInfo+=",positive";
        }
        else if(0==commandInfo[1])
        {
            paraIn.isPositiveDirection=false;
            tmpCommandInfo+=",reverse";
        }
        else
        {
            qDebug()<<"error, commandInfo[1]"<<commandInfo[1];
            isParameterOk=false;
            errorParameterIndex=1;
        }
        //提取location
        if(1==commandInfo[2])
        {
            paraIn.pathControlStrategy=E_PATH_CONTROL_STRATEGY_MCCRUM;
            tmpCommandInfo+=",mccrun";
        }
        else if(0==commandInfo[2])
        {
            paraIn.pathControlStrategy=E_PATH_CONTROL_STRATEGY_DIFFERENTIAL;
            tmpCommandInfo+=",diff";
        }
        else
        {
            qDebug()<<"error, commandInfo[2]"<<commandInfo[2];
            isParameterOk=false;
            errorParameterIndex=2;
        }
        //提取attribute
        if(0==commandInfo[3])//Ｑ经过停止点
        {
            paraIn.CNT_Value=0;
            tmpCommandInfo+=",fine";
        }
        else if(1==commandInfo[3])//A 普通经过点
        {
            paraIn.CNT_Value=100;
            tmpCommandInfo+=",cnt100";
        }
        else
        {
            qDebug()<<"error, commandInfo[3]"<<commandInfo[3];
            isParameterOk=false;
            errorParameterIndex=3;
        }

        //提取X
        paraIn.targetPosition.x=transToDistance(commandInfo[4],commandInfo[5]);
        tmpCommandInfo+=" ,x"+QString::number(paraIn.targetPosition.x,'f',3);
        //提取Y
        paraIn.targetPosition.y=transToDistance(commandInfo[6],commandInfo[7]);
        tmpCommandInfo+=" ,y"+QString::number(paraIn.targetPosition.y,'f',3);
        //提取R
        paraIn.targetPosition.rotate=transToAngle(commandInfo[8],commandInfo[9])*M_PI/180.0;
        tmpCommandInfo+=" ,r"+QString::number(paraIn.targetPosition.rotate,'f',3);

        //提取位置反馈类型
        if(0==commandInfo[10])
        {
            paraIn.pointSensorType=EM_MOVE_FREE_DP;      // 自由导航指令点位模式
            tmpCommandInfo+=",op";
        }
        else if(1==commandInfo[10])
        {
            paraIn.pointSensorType=EM_MOVE_FREE_VMARK;      // 自由导航指令点位模式
            tmpCommandInfo+=",vmark";
        }
        else if(2==commandInfo[10])
        {
            paraIn.pointSensorType=EM_MOVE_FREE_QR;      // 自由导航指令点位模式
            tmpCommandInfo+=",qr";
        }
        else if(3==commandInfo[10])
        {
            paraIn.pointSensorType=EM_MOVE_FREE_REFLECT;      // 自由导航指令点位模式
            tmpCommandInfo+=",reflect";
        }
        else
        {
            qDebug()<<"error, commandInfo[10]"<<commandInfo[10];
            isParameterOk=false;
            errorParameterIndex=10;
        }

        //提取位置反馈类型index
        paraIn.pointIndex=commandInfo[11];//点位类型索引,vmark类型
        tmpCommandInfo+=","+QString::number(paraIn.pointIndex);

        //提取激光滤波index
        paraIn.laserTypeNum=commandInfo[12];
        tmpCommandInfo+=",laser"+QString::number(paraIn.laserTypeNum);
        //提取vel
        paraIn.velocityRatio=commandInfo[13];
        tmpCommandInfo+=",vel"+QString::number(paraIn.velocityRatio);
        //提取detect
        if(1==commandInfo[14])
        {
                paraIn.isDetectBarrier=true; //是否探测障碍物
                paraIn.barrierRunMode=0;
                tmpCommandInfo+=",det0";
        }
        else if(2==commandInfo[14])
        {
                paraIn.isDetectBarrier=true; //是否探测障碍物
                paraIn.barrierRunMode=1;
                tmpCommandInfo+=",det1";
        }
        else if(3==commandInfo[14])
        {
                paraIn.isDetectBarrier=true; //是否探测障碍物
                paraIn.barrierRunMode=2;
                tmpCommandInfo+=",det2";
        }
        else if(4==commandInfo[14])
        {
                paraIn.isDetectBarrier=true; //是否探测障碍物
                paraIn.barrierRunMode=3;
                tmpCommandInfo+=",det3";
        }
        else if(5==commandInfo[14])
        {
                paraIn.isDetectBarrier=true; //是否探测障碍物
                paraIn.barrierRunMode=4;
                tmpCommandInfo+=",donothing";
        }
        else if(0==commandInfo[14])
        {
                paraIn.isDetectBarrier=false; //是否探测障碍物
                tmpCommandInfo+=",nodet";
        }
        else
        {
            qDebug()<<"error, commandInfo[14]"<<commandInfo[14];
            isParameterOk=false;
            errorParameterIndex=14;
        }
        //提取accuracy
        paraIn.allowAccuracy=commandInfo[15]/1000.0;//停车精度，单位米
        tmpCommandInfo+=","+QString::number(paraIn.allowAccuracy);

        //提取轨迹类型
        if(0==commandInfo[16])
        {
                paraIn.moveFreeMoveTrajectoryType=E_MOVE_FREE_MOVE_TRAJECTORY_LINE;
                tmpCommandInfo+=",line";
        }
        else if(2==commandInfo[16])
        {
                paraIn.moveFreeMoveTrajectoryType=E_MOVE_FREE_MOVE_TRAJECTORY_ROTATE;
                tmpCommandInfo+=",rotate";
        }
        else if(1==commandInfo[16])
        {
                paraIn.moveFreeMoveTrajectoryType=E_MOVE_FREE_MOVE_TRAJECTORY_SPLINE;
                tmpCommandInfo+=",spline";
        }
        else
        {
            qDebug()<<"error, commandInfo[16]"<<commandInfo[16];
            isParameterOk=false;
            errorParameterIndex=16;
        }

        //默认
        paraIn.moveFreeMode=E_MOVE_FREE_IMU;                 // 自由导航模式
        paraIn.pointArryType=E_MOVE_FREE_POINTS_ARRY_DP;    // 自由导航点位类型
        paraIn.pointsValue=0;                            // DP单点数值
        paraIn.registerPointsValue=0;                    // DPR寄存器点数值
        if(isParameterOk)
        {
            fleetService->addLaserPath(userInfo,paraIn);
        }
        else
        {
            addMsg( ENUM_MSG_ERROR,  "ModbusManager", "ModbusManager",  12412,  -1,errorParameterIndex+1);
            break;
        }

        break;
    }
    case 0xc3://暂停
    {
        tmpCommandInfo="pause";
        fleetService->pause(userInfo);
        break;
    }
    case 0xc4://运行
    {
        tmpCommandInfo="go";

        fleetService->run(userInfo);


        break;
    }
    case 0xc5://停止
    {
        tmpCommandInfo="stop";
        fleetService->stop(userInfo);
        break;
    }
    case 0xc6://清除消息并使能
    {
        tmpCommandInfo="clearMessage";
        fleetService->addClearMessage(userInfo);
        break;
    }
    case 0xc7://设置全局路径速度
    {
        tmpCommandInfo="setGlobalVelocity"+QString::number(commandInfo[1]);
        fleetService->addSetGlobalVelocity(userInfo,commandInfo[1]);
        break;
    }
    case 0xc8://充电
    {
        tmpCommandInfo="charge method"+QString::number(commandInfo[1])
                +" Time"+QString::number(commandInfo[2])
                +" Caparity"+QString::number(commandInfo[3]);
        ChargeParameter tmpChargeInfo;
        tmpChargeInfo.chargingCaparity=commandInfo[3];
        tmpChargeInfo.chargingTime=commandInfo[2];
        tmpChargeInfo.chargingFinishedType=(EM_CHARGING_FINISH_TYPE)commandInfo[1];
        fleetService->addCharge(userInfo,tmpChargeInfo);
        break;
    }
    case 0xc9://顶升机构
    {
        tmpCommandInfo="lift movetype"+QString::number(commandInfo[1])
                +" timeOutValue"+QString::number(commandInfo[2]);
        LiftParameter tmpLiftInfo;
        tmpLiftInfo.liftType=(EM_LIFT_TYPE)commandInfo[1];
        tmpLiftInfo.timeOutValue=commandInfo[2];
        fleetService->addLifter(userInfo,tmpLiftInfo);
        break;
    }
    case 0xca://滚筒机构
    {
        tmpCommandInfo="roller Direction"+QString::number(commandInfo[1])
                +" timeOutValue"+QString::number(commandInfo[2]);
        RollerParameter tmpRollerInfo;
        tmpRollerInfo.rollerDirectionType=(EM_ROLLER_DIRECTION_TYPE)commandInfo[1];
        tmpRollerInfo.timeOutValue=commandInfo[2];
        fleetService->addRoller(userInfo,tmpRollerInfo);
        break;
    }
    case 0xcb://加载地图
    {
        LoadMapParameter tmpMapInfo;
        if(mapList.contains(commandInfo[1]))
        {
            tmpMapInfo.mapName=mapList.value(commandInfo[1]);
        }
        else
        {
            tmpCommandInfo="loadMap name"+QString::number(commandInfo[1]);
            addMsg( ENUM_MSG_ERROR,  "ModbusManager", "ModbusManager",  12411,  -1,commandInfo[1]);
            break;
        }
        tmpMapInfo.x=transToDistance(commandInfo[2],commandInfo[3]);
        tmpMapInfo.y=transToDistance(commandInfo[4],commandInfo[5]);
        tmpMapInfo.r=transToAngle(commandInfo[6],commandInfo[7]);
        tmpCommandInfo="loadMap name"+tmpMapInfo.mapName
                +" ,x"+QString::number(tmpMapInfo.x)
                +" ,y"+QString::number(tmpMapInfo.y)
                +" ,r"+QString::number(tmpMapInfo.r);

        fleetService->addLoadMap(userInfo,tmpMapInfo);
        break;
    }
    case 0xcc://播放音乐
    {

        MusicParameter tmpMusic;
        if(musicList.contains(commandInfo[1]))
        {
            tmpMusic.musicFileName=musicList.value(commandInfo[1]);
        }
        else
        {
            tmpCommandInfo="music name"+QString::number(commandInfo[1]);
            addMsg( ENUM_MSG_ERROR,  "ModbusManager", "ModbusManager",  12410,  -1,commandInfo[1]);
            break;
        }
        tmpMusic.musicVolume=commandInfo[2];
        tmpMusic.musicPlayTimes=commandInfo[3];
        tmpCommandInfo="music name"+tmpMusic.musicFileName
                +" volume"+QString::number(commandInfo[2])
                +" times"+QString::number(commandInfo[3]);
        fleetService->addMusic(userInfo,tmpMusic);
        break;
    }
    case 0xcd://自动规划路径到站点
    {
        AutoDriveParameter tmpPara;
        tmpPara.targetStationId=commandInfo[1];
        tmpPara.speedRatio=commandInfo[2];
        tmpCommandInfo="autodrive";
                tmpCommandInfo+=" station"+QString::number(commandInfo[1])
                +" speed"+QString::number(commandInfo[2]);

        fleetService->addAutoDrive(userInfo,tmpPara);
        break;
    }
    case 0xce://等待
    {
        tmpCommandInfo="wait";
        fleetService->addWait(userInfo);
        break;
    }
    case 0xcf://等待di
    {
        WaitDiParameter tmpPara;
        tmpPara.diIndex=commandInfo[1];
        tmpPara.waitCondition=commandInfo[2];
        tmpCommandInfo="waitDi";
                tmpCommandInfo+=" index"+QString::number(commandInfo[1])
                +" value"+QString::number(commandInfo[2]);

        fleetService->addWaitDi(userInfo,tmpPara);
        break;
    }
    case 0xd0://setDo
    {
        SetDoParameter tmpPara;
        tmpPara.doIndex=commandInfo[1];
        tmpPara.doValue=commandInfo[2];
        tmpCommandInfo="setDo";
                tmpCommandInfo+=" index"+QString::number(commandInfo[1])
                +" value"+QString::number(commandInfo[2]);

        fleetService->addSetDo(userInfo,tmpPara);
        break;
    }
    case 0xd1://clearErrorAndRestoreRun
    {
        tmpCommandInfo="clearErrorAndRestoreRun";
        fleetService->clearErrorAndRestoreRun(userInfo);
        break;
    }
    case 0xd2://setbarrier
    {
        tmpCommandInfo="setbarrier";
        tmpCommandInfo+=QString::number(commandInfo[1]);
        E_BARRIER_RUN_MODE modeIn;
        switch (commandInfo[1]) {
        case 0:
            modeIn=E_BARRIER_RUN_MODE_NONE;
            break;
        case 1:
            modeIn=E_BARRIER_RUN_MODE_COMMON;
            break;
        case 2:
            modeIn=E_BARRIER_RUN_MODE_VMARK;
            break;
        case 3:
            modeIn=E_BARRIER_RUN_MODE_NARROW_A;
            break;
        case 4:
            modeIn=E_BARRIER_RUN_MODE_NARROW_B;
            break;
//        case 5:
//            modeIn=E_BARRIER_RUN_MODE_DONOTHING;
            break;
        default:
            break;
        }
        fleetService->setBarrierRunMode(userInfo,modeIn);
        break;
    }
    case 0xd3://
    {
        tmpCommandInfo="SETVEHICLEBORDEROFFSET mm";
                tmpCommandInfo+=" FRONT"+QString::number(commandInfo[1])
                +" BACK"+QString::number(commandInfo[2])+" LEFT"+QString::number(commandInfo[3])
                        +" RIGHT"+QString::number(commandInfo[4]);

        fleetService->setVehicleBorderOffset(userInfo,commandInfo[1]/1000.0,commandInfo[2]/1000.0
                ,commandInfo[3]/1000.0,commandInfo[4]/1000.0);
        break;
    }

    case 0xd4://shutdownAgv
    {
        tmpCommandInfo="shutDownAgvSystem";
        fleetService->shutDownAgvSystem(userInfo);
        break;
    }
    case 0xd5://
    {
        tmpCommandInfo="SETBORDERID";
                tmpCommandInfo+=QString::number(commandInfo[1]);

        fleetService->setCurrentVehicleBorderId(userInfo,commandInfo[1]);
        break;
    }
    default:
    {
        tmpCommandInfo="unknowCommandId"+QString::number(commandId);
        if(0!=commandId)
        {
            addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12409,  -1,commandId);
        }


    }
    }
    saveFleetCommand(tmpCommandInfo,commandSerialId);

    //更新序列号
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_executedCommandSerialId,commandSerialId);

    commandSerialId_old=commandSerialId;
    return 1;


}

int ModbusManager::processModbusFleetRequest_easyRegisterCommand(int modbusServerTypeIn, int modbusServerAdressIn)
{


    if(ENUM_CONTROL_FLEET!=powerManage->getRunModel()  || 1!=isUseModbusFleet)
    {
        return 1;
    }


    QString tmpCommandInfo;
    //简易及时命令。
    int commandId;
    static int commandId_old=0;
    readModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn,address_command-4,commandId);
    //调度拓展命令
    if(commandId_old!=commandId && 0!=commandId)
    {
        switch(commandId)
        {
//        case 0xc1://清除轨迹
//        {

//            tmpCommandInfo="clearPath ";
//            fleetService->clearCommand(userInfo);
//            break;
//        }
        case 1://运行
        {
            tmpCommandInfo="go";

            fleetService->run(userInfo);


            break;
        }
        case 2://停止并清除路径
        {
            tmpCommandInfo="stop";
            fleetService->stop(userInfo);
            writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_passedStationId+9,0);
            break;
        }
        case 3://暂停
        {
            tmpCommandInfo="pause";
            fleetService->pause(userInfo);
            break;
        }
        case 4://清除消息并使能
        {
            tmpCommandInfo="clearMessage";
            fleetService->addClearMessage(userInfo);
            break;
        }
        case 0xd1://clearErrorAndRestoreRun
        {
            tmpCommandInfo="clearErrorAndRestoreRun";
            fleetService->clearErrorAndRestoreRun(userInfo);
            break;
        }
        default:
        {
            qDebug()<<"ModbusManager::processModbusFleetRequest_easyRegisterCommand unknown commandId"<<commandId;
            addMsg( ENUM_MSG_REMIND,  "ModbusManager", "ModbusManager",  12419,  commandId);
        }
        }
        commandId_old=commandId;
        saveFleetCommand(tmpCommandInfo,0);
        writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_passedStationId+10,commandId);
    }
    if(0==commandId)
    {
        commandId_old=commandId;
    }

    //简易目标站点命令。
    int targetStationId;
    static int targetStationId_old=0;
    readModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn,address_command-3,targetStationId);
    //调度拓展命令
    if(targetStationId_old!=targetStationId && 0!=targetStationId)
    {
        AutoDriveParameter tmpPara;
        tmpPara.targetStationId=targetStationId;
        tmpPara.speedRatio=100;
        tmpCommandInfo="autodrive";
                tmpCommandInfo+=" station"+QString::number(targetStationId)
                +" speed"+QString::number(100);

        fleetService->addAutoDrive(userInfo,tmpPara);
        saveFleetCommand(tmpCommandInfo,0);
        writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_passedStationId+9,targetStationId);
    }
    targetStationId_old=targetStationId;


    return 1;
}

int ModbusManager::sendBasicInfo(int modbusServerTypeIn, int modbusServerAdressIn)
{
//    if(0==modbusServerTypeIn)
//    {
//        qDebug() << "ModbusManager::initialModbusServer ====暂时不支持";
//        return -1;
//    }

    if(NULL==fleetService)
    {
        qDebug() << "error,ModbusManager::sendBasicInfo  NULL==fleetService";
        return -1;
    }




    BatteryInformation batteryInfoOut;
    powerManage->getBatteryInfo(batteryInfoOut);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_batteryLeftLightModeRatio,
                                  (unsigned char)(batteryInfoOut.batteryLeftLightmodRatio*100));

    int stateOut=powerManage->getEngineStatue();
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_basicRunState,
                                  (unsigned char)(stateOut));

    int errorCode,warnCode, noticeCode;
    MessageLog::getInstance()->getMessageInfoCode(errorCode,warnCode, noticeCode);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_message_errorCode,
                                  errorCode);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_message_warnCode,
                                  warnCode);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_message_noticeCode,
                                  noticeCode);


//    unsigned char errorCode_high,errorCode_low,warnCode_high,warnCode_low,noticeCode_high,noticeCode_low;
//    errorCode_high=(errorCode&0xff00)>>8;
//    errorCode_low=(errorCode&0xff);
//    warnCode_high=(warnCode&0xff00)>>8;
//    warnCode_low=(warnCode&0xff);
//    noticeCode_high=(noticeCode&0xff00)>>8;
//    noticeCode_low=(noticeCode&0xff);
//    writeModbusRegister_TcpServer(modbusServerTypeIn,modbusServerAdressIn, address_message_errorCode,
//                                  errorCode_high);
//    writeModbusRegister_TcpServer(modbusServerTypeIn,modbusServerAdressIn, address_message_errorCode+1,
//                                  errorCode_low);
//    writeModbusRegister_TcpServer(modbusServerTypeIn,modbusServerAdressIn, address_message_warnCode,
//                                  warnCode_high);
//    writeModbusRegister_TcpServer(modbusServerTypeIn,modbusServerAdressIn, address_message_warnCode+1,
//                                  warnCode_low);
//    writeModbusRegister_TcpServer(modbusServerTypeIn,modbusServerAdressIn, address_message_noticeCode,
//                                  noticeCode_high);
//    writeModbusRegister_TcpServer(modbusServerTypeIn,modbusServerAdressIn, address_message_noticeCode+1,
//                                  noticeCode_low);

    beatheartRegister++;
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_heartBeat,
                                  beatheartRegister);
    VehiclePosition targetOut;
    int pointIndexOut;
    powerManage->getTargetCommandVehiclePosition(targetOut, pointIndexOut);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_currentPointIndex,
                                  pointIndexOut);
    VirtualLineDebugInfo virtualInfoOut;
    UserInfo userInfo;
    powerManage->getVirtualLineControlDebugInfo(userInfo,virtualInfoOut);

    int movingState=0;
    if(fabs(virtualInfoOut.absoluteDistance)<targetArriveDistanceError && fabs(virtualInfoOut.absoluteAngle)<targetArriveAngleError)
    {
        movingState=1;
    }
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_currentPointMovingState,
                                  movingState);
    VehiclePosition positionOut;
    fleetService->getCurrentAgvPosition(userInfo,positionOut);
    unsigned short pos_xHigh,pos_xLow,pos_yHigh,pos_yLow,pos_rHigh,pos_rLow;
    int tmpD=positionOut.x*100000;
    pos_xLow=tmpD&0xffff;
    pos_xHigh=tmpD>>16;
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_currentPos_x,pos_xHigh);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_currentPos_x+1,pos_xLow);
    tmpD=positionOut.y*100000;
    pos_yLow=tmpD&0xffff;
    pos_yHigh=tmpD>>16;
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_currentPos_y,pos_yHigh);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_currentPos_y+1,pos_yLow);
    tmpD=100*180*positionOut.rotate/M_PI;
    pos_rLow=tmpD&0xffff;
    pos_rHigh=tmpD>>16;
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_currentPos_r,pos_rHigh);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_currentPos_r+1,pos_rLow);

    E_AGV_STATE agvState;
    fleetService->getCurrentAgvState(userInfo,agvState);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_fleetStatus,(int)agvState);

    bool isLoaded;
    fleetService->getLoadedState(userInfo,isLoaded);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_loadStatus,isLoaded);

    double lineDistance,angleDistance;
    fleetService->getTargetPositionDistanceLeft(lineDistance,angleDistance);
    tmpD=100*lineDistance;
    pos_rLow=tmpD&0xffff;
    pos_rHigh=tmpD>>16;
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_currentPointRemainDistance,pos_rHigh);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_currentPointRemainDistance+1,pos_rLow);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_currentPointRemainAngle,100*angleDistance);

    double lineSpeed,rotateSpeed;
    fleetService->getCurrentAgvSpeed(userInfo,lineSpeed,rotateSpeed);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_lineSpeed,lineSpeed*1000.0);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_angleSpeed,100*180.0*rotateSpeed/M_PI);

    PathMovingInfo pathInfoOut;
    fleetService->getPathMovingInfo(pathInfoOut);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_passedStationId,pathInfoOut.passedStationId);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_passedStationId+1,pathInfoOut.currentStationId);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_passedStationId+2,pathInfoOut.nextStationId);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_passedStationId+3,pathInfoOut.passedPathId);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_passedStationId+4,pathInfoOut.currentPathId);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_passedStationId+5,pathInfoOut.nextPathId);

    std::string mapName;
    fleetService->getCurrentMapName(mapName);
    QString currentMapName=QString::fromStdString(mapName);
    int tmpMapId=-1;
    for( int i=0;i<mapList.size();i++ )
    {
        if(mapList[i]==currentMapName)
        {
            tmpMapId=i;
        }
    }
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_passedStationId+6,tmpMapId);
    int version=fleetService->getMapVersion();
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_passedStationId+7,version);
    //获取di
    unsigned short tmpDiList=fleetService->getDi_short(0);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_passedStationId+8,tmpDiList);
    //获取do
    unsigned short tmpDoList=fleetService->getDo_short(1);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_passedStationId+11,tmpDoList);
    E_LIFT_STATUS liftStatus;
    fleetService->getLiftMechanismStatus(liftStatus);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_passedStationId+12,liftStatus);
    E_ROLLER_STATUS rollerStatus;
    fleetService->getRollerMechanismStatus(rollerStatus);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_passedStationId+13,rollerStatus);

    int tmpFloorId=fleetService->getCurrentFloorId();
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_passedStationId+14,tmpFloorId);
    EN_CONTROL_TYPE agvMode=fleetService->getRunModel();
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_passedStationId+15,agvMode);

    int tmpBorderId;
    fleetService->getCurrentVehicleBorderId(userInfo,tmpBorderId);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_passedStationId+16,tmpBorderId);
    E_BARRIER_TETECT_MODE barriermodeOut;
    fleetService->getBarriorDetectMode(barriermodeOut);
    writeModbusRegister_Server(modbusServerTypeIn,modbusServerAdressIn, address_passedStationId+17,barriermodeOut);

    return 1;
}

int ModbusManager::exchangeRobotRegister()
{
    for(int i=0;i<robotModbusServerInfos.size();i++)
    {
        if(robotModbusServerInfos[i].isValid)
        {
            //写R
            int tmpValue;
            short sValue;
            for(int n=robotModbusServerInfos[i].robotWriteStartIndex;n<robotModbusServerInfos[i].robotWriteEndIndex;n++)
            {
                readModbusRegister_Server(true,robotModbusServerInfos[i].port,n,tmpValue);
                sValue=tmpValue;
                powerManage->setRegisterValue_inner(robotModbusServerInfos[i].robotId,0,n,0,sValue);
            }



            //读R
             bool initialStatus=false;
             QVector<double> regOut;

             if(1==powerManage->returnRValue(robotModbusServerInfos[i].robotId,initialStatus,regOut) && true==initialStatus)
             {
                 if(robotModbusServerInfos[i].robotReadStartIndex<0||robotModbusServerInfos[i].robotReadEndIndex>=regOut.size())
                 {
                     addMsg( ENUM_MSG_ERROR,  "ModbusManager", "ModbusManager",  12432, robotModbusServerInfos[i].robotId,
                              robotModbusServerInfos[i].robotReadStartIndex,
                             robotModbusServerInfos[i].robotReadEndIndex,regOut.size());
                     return 0;
                 }
                 for(int n=robotModbusServerInfos[i].robotReadStartIndex;n<robotModbusServerInfos[i].robotReadEndIndex;n++)
                 {

                     writeModbusRegister_Server(true,robotModbusServerInfos[i].port, n,regOut[n]);
                 }
             }



            if(false==startFinishedStatus)
            {
                addMsg( ENUM_MSG_ERROR,  "ModbusManager", "ModbusManager",  12431, robotModbusServerInfos[i].robotId,robotModbusServerInfos[i].port);
            }
        }


    }
    return 1;
}



int ModbusManager::initialModbusServer(int modbusServerType, QString modbusServerAdressIn)
{
    if(0==modbusServerType)
    {
        createModbusComServerSlot(modbusServerAdressIn.toInt());
        if(true==startFinishedStatus)
        {
            return 1;
        }
    }
    else if(1==modbusServerType)
    {
//        return createModbusTcpServer(modbusServerAdressIn.toInt());//会柱塞主线程
        createModbusTcpServerSlot(modbusServerAdressIn.toInt(),D_MODBUS_REGISTER_SIZE);
        if(true==startFinishedStatus)
        {
            return 1;
        }

    }
    return -1;

}

int ModbusManager::initialRobotModbusServer()
{
    for(int i=0;i<robotModbusServerInfos.size();i++)
    {
        if(robotModbusServerInfos[i].isValid)
        {
            createModbusTcpServerSlot(robotModbusServerInfos[i].port,robotModbusServerInfos[i].regSize);
            if(false==startFinishedStatus)
            {
                addMsg( ENUM_MSG_ERROR,  "ModbusManager", "ModbusManager",  12431, robotModbusServerInfos[i].robotId,robotModbusServerInfos[i].port);
            }
        }


    }
    return 1;


}

int ModbusManager::initialConfigFile(QString configFileName)
{
    qDebug() << "ModbusManager::initialConfigFile ==== 初始化配置文件：" << configFileName;

    DomParser domparser;
    bool ok = domparser.openXml(configFileName, QIODevice::ReadOnly);
    if( !ok )
    {

        qDebug() << " 配置文件打开失败：" << configFileName;
        return -1;
    }

    QDomNode domNode = domparser.findSubNode(domparser.getRootDomElement(), "modbusTcpServerDafulatId", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(domNode, modbusTcpServerDafulatId);

     domNode = domparser.findSubNode(domparser.getRootDomElement(), "timerPeriod", ok );
    if( !ok )
    {

        qDebug() << "timerPeriod no fined,initialConfigFile failed " << configFileName;
        return -1;
    }
    domparser.readXml(domNode, timerPeriod);

    QDomNode domNode_map = domparser.findSubNode(domparser.getRootDomElement(), "systemInfoMap", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }

    QDomNode tmpNode = domparser.findSubNode(domNode_map, "isUseSystemInfoMap", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(tmpNode, isUseSystemInfoMap);

    tmpNode = domparser.findSubNode(domNode_map, "modbusServerType", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(tmpNode, modbusServerType);

    tmpNode = domparser.findSubNode(domNode_map, "modbusServerAdress", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(tmpNode, modbusServerAdress);

    tmpNode = domparser.findSubNode(domNode_map, "address_batteryLeftLightModeRatio", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(tmpNode, address_batteryLeftLightModeRatio);

    tmpNode = domparser.findSubNode(domNode_map, "address_currentPointIndex", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(tmpNode, address_currentPointIndex);

    tmpNode = domparser.findSubNode(domNode_map, "address_currentPointMovingState", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(tmpNode, address_currentPointMovingState);

//    tmpNode = domparser.findSubNode(domNode_map, "address_currentPointRemainDistance", ok );
//    if( !ok )
//    {

//        qDebug() << "initialConfigFile failed" << configFileName;
//        return -1;
//    }
//    domparser.readXml(tmpNode, address_currentPointRemainDistance);

//    tmpNode = domparser.findSubNode(domNode_map, "address_currentPointRemainAngle", ok );
//    if( !ok )
//    {

//        qDebug() << "initialConfigFile failed" << configFileName;
//        return -1;
//    }
//    domparser.readXml(tmpNode, address_currentPointRemainAngle);

    tmpNode = domparser.findSubNode(domNode_map, "address_heartBeat", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(tmpNode, address_heartBeat);


    tmpNode = domparser.findSubNode(domNode_map, "address_message_errorCode", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(tmpNode, address_message_errorCode);

    tmpNode = domparser.findSubNode(domNode_map, "address_message_warnCode", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(tmpNode, address_message_warnCode);

    tmpNode = domparser.findSubNode(domNode_map, "address_message_noticeCode", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(tmpNode, address_message_noticeCode);

    tmpNode = domparser.findSubNode(domNode_map, "address_basicRunState", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(tmpNode, address_basicRunState);

    tmpNode = domparser.findSubNode(domNode_map, "address_command", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(tmpNode, address_command);

    tmpNode = domparser.findSubNode(domNode_map, "targetArriveDistanceError", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(tmpNode, targetArriveDistanceError);

    tmpNode = domparser.findSubNode(domNode_map, "targetArriveAngleError", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(tmpNode, targetArriveAngleError);

    tmpNode = domparser.findSubNode(domNode_map, "doRegisterValidMask", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed ,doRegisterValidMask" << configFileName;
        return -1;
    }
    domparser.readXml(tmpNode, doRegisterValidMask);

    //fleet
    domNode_map = domparser.findSubNode(domparser.getRootDomElement(), "modbusFleetInfo", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }

    tmpNode = domparser.findSubNode(domNode_map, "isUseModbusFleet", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(tmpNode, isUseModbusFleet);

    tmpNode = domparser.findSubNode(domNode_map, "maxFleetCatchLineForDebug", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    domparser.readXml(tmpNode, maxFleetCatchLineForDebug);

    //musicinfo
    domNode_map = domparser.findSubNode(domparser.getRootDomElement(), "musicInfo", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    QDomNodeList devNodes = domNode_map.childNodes();
    QString tmpName;
    int tmpId;
    for(int i=0;i<devNodes.size();i++)
    {
        QDomNode devND =  devNodes.at(i);
        domparser.readXml( devND, tmpName );
        tmpId=devND.toElement().attribute("id" ).toUInt();
        musicList.insert(tmpId,tmpName);
    }

    domNode_map = domparser.findSubNode(domparser.getRootDomElement(), "mapInfo", ok );
    if( !ok )
    {

        qDebug() << "initialConfigFile failed" << configFileName;
        return -1;
    }
    devNodes = domNode_map.childNodes();
    for(int i=0;i<devNodes.size();i++)
    {
        QDomNode devND =  devNodes.at(i);
        domparser.readXml( devND, tmpName );
        tmpId=devND.toElement().attribute("id" ).toUInt();
        mapList.insert(tmpId,tmpName);
    }


    //programinfo
    domNode_map = domparser.findSubNode(domparser.getRootDomElement(), "programInfo", ok );
    if( ok )
    {
        devNodes = domNode_map.childNodes();
        for(int i=0;i<devNodes.size();i++)
        {
            QDomNode devND =  devNodes.at(i);
            domparser.readXml( devND, tmpName );
            tmpId=devND.toElement().attribute("id" ).toUInt();
            programNameList.insert(tmpId,tmpName);
        }
    }


    //　读取ModbusRtu
    domNode = domparser.findSubNode(domparser.getRootDomElement(), "ModbusRtu", ok );
    if(!ok)
    {

            qDebug() << " 读取ModbusRtu失败：" << configFileName;

    }

    //　读取Master0
    QDomNode tmpComNode = domparser.findSubNode(domNode, "Master0", ok );
    if(!ok)
    {
       qDebug() << " 读取Master0失败：" << configFileName;
    }


    //　读取串口配置参数结点
    domNode = domparser.findSubNode(tmpComNode, "SerialSetting", ok );
    if(!ok)
    {
         qDebug() << " 读取SerialSetting失败：" << configFileName;
    }

    comParameter_master0.modbusType=E_MODBUS_TYPE_RTU;


    // 读取串口设备文件
    QString tmpDeviceName;
    domparser.readXml(domparser.findSubNode(domNode, "serialDeviceName", ok),\
                      tmpDeviceName );
    if( !ok  )
    {
        qDebug() << " 读取SerialSetting失败：" << configFileName;
        return -3;
    }
    comParameter_master0.serialDeviceName="/dev/";
    comParameter_master0.serialDeviceName+=tmpDeviceName;


    // 读取isUseUsb
    domparser.readXml(domparser.findSubNode(domNode, "isUseUsbDeviceSerialCode", ok),\
                      comParameter_master0.isUseUsbDeviceSerialCode );
    if( !ok  )
    {
        qDebug() << " 读取SerialSetting失败：" << configFileName;
        return -3;
    }


    // 读取配置文件波特率
    int baud;
    domparser.readXml(domparser.findSubNode(domNode, "baud" ,ok), baud);
    if( !ok )
    {
        qDebug() << " 读取SerialSetting失败：" << configFileName;
        return -4;
    }
    comParameter_master0.baud = baud;


    // 读取配置文件校验位
    QString parity;
    int parityNumber;
    domparser.readXml(domparser.findSubNode(domNode, "parity", ok), parity);
    if( !ok )
    {
        qDebug() << " 读取SerialSetting失败：" << configFileName;
        return -5;
    }

    if(parity == "NONE")
    {
        comParameter_master0.parity = 'N';
    }
    else if (parity == "EVEN")
    {
        comParameter_master0.parity = 'E';
    }
    else if (parity == "ODD")
    {
        comParameter_master0.parity = 'O';
    }
    else
    {
        qDebug()  << "BatteryMonitor::initialBatteryByConfigFile ==== 校验位设置错误:" \
                  << configFileName;
        return -6;
    }


    //　读取配置文件数据位
    domparser.readXml(domparser.findSubNode(domNode, "data_bit", ok), comParameter_master0.data_bit);
    if( !ok )
    {
        qDebug() << " 读取SerialSetting失败：" << configFileName;
        return -7;
    }



    // 读取配置文件停止位
    domparser.readXml(domparser.findSubNode(domNode, "stop_bit", ok), comParameter_master0.stop_bit );
    if( !ok )
    {
        qDebug() << " 读取SerialSetting失败：" << configFileName;
        return -8;
    }


    //RobotModbusServerInfo
    QDomNode domNode_robotmodbus = domparser.findSubNode(domparser.getRootDomElement(), "RobotModbusServerInfo", ok );
    if( !ok )
    {

        qDebug() << "RobotModbusServerInfo not find,initialConfigFile failed" << configFileName;
        return -1;
    }
    QDomNodeList robotNodes = domNode_robotmodbus.childNodes();
    QDomNode tmpDomNode;
    RobotModbusServerInfo tmpRobotInfo;
    for(int i=0;i<robotNodes.size();i++)
    {
        tmpDomNode=domparser.findSubNode(robotNodes.at(i), "isValid", ok );
        if( !ok )
        {
            qDebug() << "RobotModbusServerInfo not find,sub node not find" << i;
            return -1;
        }
        domparser.readXml( tmpDomNode, tmpRobotInfo.isValid );

        tmpDomNode=domparser.findSubNode(robotNodes.at(i), "robotId", ok );
        if( !ok )
        {
            qDebug() << "RobotModbusServerInfo not find,sub node not find" << i;
            return -1;
        }
        domparser.readXml( tmpDomNode, tmpRobotInfo.robotId );

        tmpDomNode=domparser.findSubNode(robotNodes.at(i), "robotReadStartIndex", ok );
        if( !ok )
        {
            qDebug() << "RobotModbusServerInfo not find,sub node not find" << i;
            return -1;
        }
        domparser.readXml( tmpDomNode, tmpRobotInfo.robotReadStartIndex );

        tmpDomNode=domparser.findSubNode(robotNodes.at(i), "robotReadEndIndex", ok );
        if( !ok )
        {
            qDebug() << "RobotModbusServerInfo not find,sub node not find" << i;
            return -1;
        }
        domparser.readXml( tmpDomNode, tmpRobotInfo.robotReadEndIndex );

        tmpDomNode=domparser.findSubNode(robotNodes.at(i), "robotWriteStartIndex", ok );
        if( !ok )
        {
            qDebug() << "RobotModbusServerInfo not find,sub node not find" << i;
            return -1;
        }
        domparser.readXml( tmpDomNode, tmpRobotInfo.robotWriteStartIndex );

        tmpDomNode=domparser.findSubNode(robotNodes.at(i), "robotWriteEndIndex", ok );
        if( !ok )
        {
            qDebug() << "RobotModbusServerInfo not find,sub node not find" << i;
            return -1;
        }
        domparser.readXml( tmpDomNode, tmpRobotInfo.robotWriteEndIndex );

        tmpDomNode=domparser.findSubNode(robotNodes.at(i), "regSize", ok );
        if( !ok )
        {
            qDebug() << "RobotModbusServerInfo not find,sub node not find" << i;
            return -1;
        }
        domparser.readXml( tmpDomNode, tmpRobotInfo.regSize );

        tmpDomNode=domparser.findSubNode(robotNodes.at(i), "port", ok );
        if( !ok )
        {
            qDebug() << "RobotModbusServerInfo not find,sub node not find" << i;
            return -1;
        }
        domparser.readXml( tmpDomNode, tmpRobotInfo.port );

        robotModbusServerInfos.append(tmpRobotInfo);

    }


    domparser.closeXml();
    return 1;

}

void ModbusManager::addMsg(int messageLevel, string componentName, string messageType,
                           int messageCode, int robotId,
                           int parameter1, int parameter2, int parameter3, int parameter4)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;


    QString infomationStr;

    switch(messageCode)
    {
    case 12432:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：交换寄存器(%1~%2)超出寄存器范围%3！")
                .arg(parameter1).arg(parameter2).arg(parameter3);
        break;
    }
    case 12431:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：启动机器人%1寄存器modbus服务(port%2)失败！")
                .arg(parameter1).arg(parameter2);
        break;
    }
    case 12430:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：手动结束！")
                .arg(parameter1);
        break;
    }
    case 12429:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：切换为调度模式！")
                .arg(parameter1);
        break;
    }
    case 12428:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：切换为外部模式！")
                .arg(parameter1);
        break;
    }
    case 12427:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：切换为T2手动！")
                .arg(parameter1);
        break;
    }
    case 12426:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：切换为T1手动！")
                .arg(parameter1);
        break;
    }
    case 12425:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：关闭避障碍检测！")
                .arg(parameter1);
        break;
    }
    case 12424:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：开启避障碍检测！")
                .arg(parameter1);
        break;
    }
    case 12423:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：jog手动开始！")
                .arg(parameter1);
        break;
    }
    case 12422:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：未知手动方向！")
                .arg(parameter1);
        break;
    }
    case 12421:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：加载内部程序号%1失败，序号不存在配置表中！")
                .arg(parameter1);
        break;
    }
    case 12420:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：加载内部程序号%1！")
                .arg(parameter1);
        break;
    }
    case 12419:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：未知简易命令%1！")
                .arg(parameter1);
        break;
    }
    case 12418:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：打开Modbus串口失败！")
                .arg(parameter1);
        break;
    }
    case 12417:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：写远程ModbusRtu服务器寄存器失败！")
                .arg(parameter1);
        break;
    }
    case 12416:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：读远程ModbusRtu服务器寄存器失败！")
                .arg(parameter1);
        break;
    }
    case 12415:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：写远程ModbusTcp服务器寄存器失败！")
                .arg(parameter1);
        break;
    }
    case 12414:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：读远程ModbusTcp服务器寄存器失败！")
                .arg(parameter1);
        break;
    }
    case 12413:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：与远程ModbusTcp服务器通讯失败！")
                .arg(parameter1);
        break;
    }
    case 12412:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：轨迹命令的第%1号参数超出正常范围！")
                .arg(parameter1);
        break;
    }
    case 12411:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：%1号地图不存在！")
                .arg(parameter1);
        break;
    }
    case 12410:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：%1号音乐不存在！")
                .arg(parameter1);
        break;
    }
    case 12409:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：未支持的命令字%1！")
                .arg(parameter1);
        break;
    }
    case 12408:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：初始化小车位置,使用Ｐ[%1]！")
                .arg(parameter1);
        break;
    }
    case 12407:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：停止程序！")
                .arg(parameter1);
        break;
    }
    case 12406:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：暂停程序！")
                .arg(parameter1);
        break;
    }
    case 12405:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：运行程序！")
                .arg(parameter1);
        break;
    }
    case 12404:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：报警复位！")
                .arg(parameter1);
        break;
    }
    case 12403:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：使能关！")
                .arg(parameter1);
        break;
    }
    case 12402:
    {
        infomationStr =  QObject::tr("ModbusManager模块控制：使能开！")
                .arg(parameter1);
        break;
    }
    case 12401:
    {
        infomationStr =  QObject::tr("ModbusManager模块启动ModbusServer失败！")
                .arg(parameter1);
        break;
    }
    case 12400:
    {
        infomationStr =  QObject::tr("ModbusManager模块初始化失败！")
                .arg(parameter1);
        break;
    }
    default:

        break;
    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    MessageLog::getInstance()->addMessage(tmpMsg);
}


